# -- coding: utf-8 --
"""
# [Example] Miahx© 交互框架信息表示模型的 DDS 消息收发示例

**模块：MIAHX© / 框架基础服务层（FFST） / 交互框架信息表示模型（IFIRM）**

Version: 0.4.10,
Last updated: October 17, 2024

## Contributor(s)
* YAO Naiming <[naiming@iscas.ac.cn](mailto:naiming@iscas.ac.cn)>

## Change log
### v0.4.10
- .17: [更新] 测试 EXT_NAVIGATION_PC 主题
- .9: [更新] 修复缺陷，支持 packed 消息写入（timestamps 和 sns）(deprecated)
- .2: [更新] 适配新接口
### v0.4.7
- .24: [新增] 初始实现

---
Copyright © 2024-2025 MIAHX Contributors, ISCAS, and ASIC. All Rights Reserved.
Licensed under Apache-2.0.
"""

import threading
import time

import numpy as np
from miahx_support.dds.fastdds import *
from miahx_support.stag import STag

import launcher


def bng_lidar_writer(p: Participant, e: threading.Event):
    stag = STag(**dict(sensor_channel=1, sensor_domain='CARRIER', sensor_type='VBNG_LIDAR',
                       sensor_channel_type='IGNORE', sensor_modality='POINT_CLOUD',
                       sensor_data_format='NPZ')).encode()
    writer = PointCloudStreamWriter(p, topic, stag=stag, srate=2, schns=['X', 'Y', 'Z'])
    while not e.is_set():
        chunk = [(1, 1.0,
                  np.array([[1.1, 2.1, 3.1], [4.1, 5.1, 6.1]], dtype=np.float32),
                  np.array([[7, 8, 9], [10, 11, 12]], dtype=np.uint8)),
                 (2, 2.0,
                  np.array([[11.1, 12.1, 13.1], [14.1, 15.1, 16.1]], dtype=np.float32),
                  np.array([[17, 18, 19], [110, 111, 112]], dtype=np.uint8))]
        # sample = [dict(points=np.zeros((2000000, 3), dtype=np.float32),
        #                colours=np.zeros((2000000, 4), dtype=np.uint8))]
        writer.write(chunk, iid=0, uts=0)
        time.sleep(1)


def bng_lidar_reader(p: Participant, e: threading.Event):
    def on_bng_lidar_available(data):
        hdr = data.header()
        dm = hdr.header().domain()
        src = hdr.header().source()
        prefix = f'[{dm}::{src}::{name}]'

        schns = tuple(hdr.schns())
        samples = list(data.samples())
        log.info(f'{prefix} stag = `{hdr.stag()}`')
        log.info(f'{prefix} srate = `{hdr.srate()}`')
        log.info(f'{prefix} schns = `{schns}`')
        log.info(f'{prefix} elements = [{len(samples)}]')

        for i, sample in enumerate(samples):
            log.info(f'{prefix} Element [{i}]:')
            points = np.asarray(sample.points()).reshape((-1, len(schns)))
            colours = np.asarray(sample.colours()).reshape((-1, 3))
            log.info(f'{prefix} \t\tpoints.shape = `{points.shape}`')
            log.info(f'{prefix} \t\tcolours.shape = `{colours.shape}`')
            log.info(f'{prefix} End Element [{i}] --------')

        log.info(f'{prefix} ----------------')

    with PointCloudStreamReader(p, topic, on_data_listeners=[on_bng_lidar_available]) as reader:
        name = type(reader).__name__
        log.success(f'[{name}] Topic: {reader.topic_name}')
        reader.wait(e)


if __name__ == '__main__':
    log = launcher.log
    event = threading.Event()
    participant = Participant()

    # topic = Topics.BNG_LIDAR_PC
    topic = Topics.CAR_TRAJECTORY_PC

    launcher.run(lambda: (event, [
        # threading.Thread(target=bng_lidar_writer, args=(participant, event), daemon=False),
        threading.Thread(target=bng_lidar_reader, args=(participant, event), daemon=False),
    ]))
